#include <Arduino.h>
#include <Ticker.h>

#include "light/LightUdpClient.h"
#include "cam/Cam.hpp"
#include "quadruped/Quadruped.hpp"

#define indexCheck(functionIndex, top) if (functionIndex < 0 || functionIndex >= top) { \
    return;                                                                                        \
}

#define indexCheckNext(functionIndex, top) if (functionIndex < 0 || functionIndex >= top) { \
    continue;                                                                                        \
}


const int streamPort = 8004;
const int serverPort = 8081;
const uint16_t localUdpPort = 2333;

LightUdpClient lightClient;
MsgHeader msgHeader;

Ticker ticker;

Cam cam;

Quadruped quadruped = Quadruped(
        {
                .leftLimit = QUADRUPED_LEFT_LIMIT,
                .rightLimit = QUADRUPED_RIGHT_LIMIT,
                .hipResetPos = QUADRUPED_HIP_RESET_POS,

                .upperLimit = QUADRUPED_UPPER_LIMIT,
                .lowerLimit = QUADRUPED_LOWER_LIMIT,
                .kneeResetPos = QUADRUPED_KNEE_RESET_POS,
                .kneeConvergence = QUADRUPED_KNEE_CONVERGENCE,

                .stepHigh = QUADRUPED_STEP_HIGH,
                .stepLength = QUADRUPED_STEP_LENGTH,
                .direction = {QUADRUPED_SERVO_DIRECTION}
        }
);


// opCode 操作表

void debugOp();

void servoTurn();

void action();

void allPinAction();

void robotSettings();

void camGeneralOp();

void camSettingOp();

void setCamChannelIndex();

void turnLimit();

#define OP_TAB_LEN 9

void (*opTab[OP_TAB_LEN])() = {debugOp, servoTurn, action, allPinAction, camGeneralOp,
                      camSettingOp, setCamChannelIndex, robotSettings, turnLimit};

// 消息控制线程
[[noreturn]]
void receiveMessage(void *);

// 机器人动作线程
void quadrupedActionTick();

void setup() {
    Serial.begin(115200);
    while (!Serial) {
    }
    serial_info("init quadruped!");

    // esp32s3默认 8 和 9 其它引脚也可指定。查看当前日志请开启至少info级别的日志。
    log_i(" sda is %d ,scl is %d", PWM_SDA, PWM_SCL);
    quadruped.init(PWM_SDA, PWM_SCL);
    // 初始化摄像头，并将设备属性中加入摄像头已有参数
    int attrCount = 4;
    String attr = cam.begin(&attrCount, String("version:3.0\n"
                                               "devType:CamQuadruped\n"
                                               // setCamChannelIndex函数指针的索引，摄像头类型的设备需要带此属性，用于light server回调
                                               "setCamChannelIndexOpCode:6\n"
                                               /* 上下云台最大偏转值 60 可选范围 1 ~ 100。防止舵机被阻挡，如果安装角度较低，
                                                且上方无遮挡可以适当加大，以获得更大转动范围 */
                                               "maxUpDownGimbal:60\n"
    ));
    char attr_[attr.length() + 1];
    attr.toCharArray(attr_, attr.length() + 1);
    Serial.println(attr_);

    ConnectInfo connectInfo;
    connectInfo.ssid = WIFI_SSID;
    connectInfo.passphrase = WIFI_PASSWORD;
    connectInfo.serverPort = serverPort;
    connectInfo.attrCount = attrCount;
    connectInfo.devAttr = attr_;

#ifdef SERVER_HOST
    connectInfo.host = SERVER_HOST;
#endif
    connectInfo.userName = USER_NAME;
    connectInfo.passwd = PASS_WD;
    const IPAddress &address = lightClient.connectServer(&connectInfo);
    // 控制通道连接成功才可打开 视频帧通道
    delay(1000);
    cam.createChannel(address, streamPort, localUdpPort);
    log_i("start action ticker");
    Serial.println("start action ticker");
    ticker.attach_ms(step_delay, quadrupedActionTick);

    xTaskCreatePinnedToCore(receiveMessage, "receiveMessage", 10000,
                            NULL, 0, NULL, 0);

}

void loop() {
    cam.sendVideo();
    delay(1000);
}

/**
 * 控制动作的线程，定时器，线程中不能使用delay.
 * 所以每一步需要切换执行内容
 * */
void quadrupedActionTick() {

    int action = quadruped.getQuadrupedAction();
    if (action == -1) {
        return;
    }
    quadruped.doAsyncAction(action);
}

[[noreturn]] void receiveMessage(void *) {
    Serial.println("do receive");
    while (true) {
        lightClient.startHeartbeat();
        int result = lightClient.readMsgHeader(&msgHeader);

        if (result == MSG_FAILED) {
            continue;
        }
        int opCode = msgHeader.opCode;
        Serial.printf("cmdId:%d,opCode:%d\n", msgHeader.cmdIdVal, opCode);
        indexCheckNext(opCode, OP_TAB_LEN)
        opTab[opCode]();
    }
}

/**
 * 调试舵机使用的
 * */
void servoTurn() {
    int target = lightClient.readInt(' ');
    int pos = lightClient.readInt('\n');
    quadruped.turn(target, pos);
}

void turnLimit() {
    int target = lightClient.readInt(' ');
    int pos = lightClient.readInt('\n');
    quadruped.turnLimit(target, pos);
}

/**
 * 机器人动作,对应具体动作的序列，为连续动作
 * */
void action() {
    int action = lightClient.readInt('\n');
    indexCheck(action, 6)
    quadruped.setQuadrupedAction(action);
    Serial.printf("action is %d\n", action);
}

/**
 * 所以pin的调节 复位和停止
 * */
void allPinAction() {
    int action = lightClient.readInt('\n');
    indexCheck(action, 3)
    void (Quadruped::*allPinAction)() = quadruped.allPinActionTab[action];
    (quadruped.*allPinAction)();
}


void robotSettings() {
    int item = lightClient.readInt(' ');
    int arg = lightClient.readInt('\n');
    indexCheck(item, 5)
    void (Quadruped::*robotSetting)(uint8_t) = quadruped.robotSettings[item];
    (quadruped.*robotSetting)(arg);
}

/**
 * 摄像头无参操作函数调度 opCode 为 3 参数行是 一个数字，为一个无参函数下标
 * 开关视频
 * */
void camGeneralOp() {
    int functionIndex = lightClient.readInt('\n');
    if (functionIndex > -1 && functionIndex < 4) {
        void (Cam::*function)() = cam.generalFunTab[functionIndex];
        (cam.*function)();
    }
}

void camSettingOp() {
    int functionIndex = lightClient.readInt(' ');
    // 目标函数再次调度一张函数指针表
    int subFunctionIndex = lightClient.readInt(' ');
    indexCheck(functionIndex, 2)
    String args = lightClient.readStringUntil('\n');
    if (functionIndex > -1 && functionIndex < 2) {
        void (Cam::*function)(int, String) = cam.camSettingOp[functionIndex];
        (cam.*function)(subFunctionIndex, args);
    }
}

/**
 * light server回调设置摄像头通道
 * */
void setCamChannelIndex() {
    int channelIndex = lightClient.readInt('\n');
    cam.setChannelIndex(channelIndex);
}

void debugOp() {
    Serial.println("rec:" + lightClient.readStringUntil('\n'));
}
